#include "chess_board_pose_correct.h"

using namespace cv;
using namespace std;


void ChessBoardPoseCorrect::getProjectionMatrix(const cv::Mat& image, cv::Mat& matrix)
{
	if (image.empty()) {
		return;
	}

	vector<Point2f> srcPoints, dstPoints;
	findPointsPair(image, srcPoints, dstPoints);
	if (srcPoints.empty() || dstPoints.empty()) {
		return;
	}

	matrix = getPerspectiveTransform(srcPoints, dstPoints);
}


void ChessBoardPoseCorrect::findPointsPair(const cv::Mat& image,
											std::vector<cv::Point2f>& srcPoints,
											std::vector<cv::Point2f>& dstPoints)
{
	if (image.empty()) {
		return;
	}

	bool found = findCorners(image, srcPoints);
	if (!found) {
		return;
	}

	findFourCorners(srcPoints, dstPoints);
}


void ChessBoardPoseCorrect::findFourCorners(std::vector<cv::Point2f>& srcPoints, std::vector<cv::Point2f>& dstPoints)
{
	if (srcPoints.size() !=  9) {
		return;
	}
	
	Point2f center = srcPoints[srcPoints.size() / 2];
	srcPoints = {srcPoints[0], srcPoints[2], srcPoints[8], srcPoints[6]};

	auto distance = [](Point2f& p1, Point2f& p2) {
		return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
	};

	float boardSize = 0;
	boardSize += distance(srcPoints[0], srcPoints[1]) + distance(srcPoints[2], srcPoints[3]);
	boardSize += distance(srcPoints[0], srcPoints[3]) + distance(srcPoints[1], srcPoints[2]);
	boardSize = boardSize / srcPoints.size() / 2;

	for (auto& point : srcPoints) {
		int sx = point.x < center.x ? -1 : 1;
		int sy = point.y < center.y ? -1 : 1;
		dstPoints.push_back({ center.x + sx * boardSize, center.y + sy * boardSize });
	}
}

